/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 *
 * fsolve.c
 *
 * Code generation for function 'fsolve'
 *
 */

/* Include files */
#include "fsolve.h"
#include "Modalmass_estimation_newchangefinal1219_internal_types.h"
#include "Modalmass_estimation_newchangefinal1219_rtwutil.h"
#include "checkStoppingCriteria.h"
#include "computeFiniteDifferences.h"
#include "linearLeastSquares.h"
#include "rt_nonfinite.h"
#include "rt_nonfinite.h"
#include <math.h>

/* Function Definitions */
double fsolve(const double fun_workspace_f[3], double x[4], double fval[4],
              double *output_iterations, double *output_funcCount,
              double *output_stepsize, double *output_firstorderopt,
              char output_algorithm[19])
{
  static const char cv[19] = {'l', 'e', 'v', 'e', 'n', 'b', 'e', 'r', 'g', '-',
                              'm', 'a', 'r', 'q', 'u', 'a', 'r', 'd', 't'};
  static const signed char varargin_1[4] = {2, 5, 8, 6};
  c_struct_T FiniteDifferences;
  c_struct_T b_FiniteDifferences;
  double augJacobian[32];
  double JacCeqTrans[16];
  double rhs[8];
  double a__3[4];
  double dx[4];
  double f_temp[4];
  double gradf[4];
  double b_f_temp_tmp;
  double b_gamma;
  double c;
  double c_f_temp_tmp;
  double exitflag;
  double f_temp_tmp;
  double funDiff;
  double normGradF;
  double relFactor;
  double resnorm;
  double t;
  int aIdx;
  int bIdx;
  int b_exitflag;
  int funcCount;
  int iter;
  boolean_T evalOK;
  boolean_T exitg1;
  boolean_T stepSuccessful;
  for (aIdx = 0; aIdx < 19; aIdx++) {
    output_algorithm[aIdx] = cv[aIdx];
  }
  funDiff = rtInf;
  iter = 0;
  /*  确保 F 是 1x4 实数数组 */
  /*  T2/T1 输入 */
  /*  T3/T1 输入 */
  f_temp_tmp = fun_workspace_f[0] / fun_workspace_f[1];
  f_temp[0] = f_temp_tmp - 0.32391053207156639;
  b_f_temp_tmp = fun_workspace_f[0] / fun_workspace_f[2];
  f_temp[1] = b_f_temp_tmp - 0.15811388300841897;
  f_temp[2] = 112.95502774425722;
  f_temp[3] = -91.215351414427687;
  resnorm = 0.0;
  for (bIdx = 0; bIdx < 4; bIdx++) {
    dx[bIdx] = rtInf;
    x[bIdx] = varargin_1[bIdx];
    c = f_temp[bIdx];
    fval[bIdx] = c;
    resnorm += c * c;
  }
  FiniteDifferences.nonlin.workspace.fun.workspace.f[0] = fun_workspace_f[0];
  FiniteDifferences.nonlin.workspace.fun.workspace.f[1] = fun_workspace_f[1];
  FiniteDifferences.nonlin.workspace.fun.workspace.f[2] = fun_workspace_f[2];
  FiniteDifferences.numEvals = 0;
  a__3[0] = 2.0;
  a__3[1] = 5.0;
  a__3[2] = 8.0;
  a__3[3] = 6.0;
  computeFiniteDifferences(&FiniteDifferences, fval, a__3, JacCeqTrans);
  funcCount = FiniteDifferences.numEvals + 1;
  b_gamma = 0.01;
  for (bIdx = 0; bIdx < 4; bIdx++) {
    b_exitflag = bIdx << 3;
    augJacobian[b_exitflag] = JacCeqTrans[bIdx];
    augJacobian[b_exitflag + 1] = JacCeqTrans[bIdx + 4];
    augJacobian[b_exitflag + 2] = JacCeqTrans[bIdx + 8];
    augJacobian[b_exitflag + 3] = JacCeqTrans[bIdx + 12];
    aIdx = (bIdx + 1) << 3;
    augJacobian[aIdx - 4] = 0.0;
    augJacobian[aIdx - 3] = 0.0;
    augJacobian[aIdx - 2] = 0.0;
    augJacobian[aIdx - 1] = 0.0;
    augJacobian[(bIdx + b_exitflag) + 4] = 0.1;
  }
  for (b_exitflag = 0; b_exitflag < 4; b_exitflag++) {
    aIdx = b_exitflag << 3;
    bIdx = b_exitflag << 2;
    JacCeqTrans[bIdx] = augJacobian[aIdx];
    JacCeqTrans[bIdx + 1] = augJacobian[aIdx + 1];
    JacCeqTrans[bIdx + 2] = augJacobian[aIdx + 2];
    JacCeqTrans[bIdx + 3] = augJacobian[aIdx + 3];
    gradf[b_exitflag] = 0.0;
  }
  for (bIdx = 0; bIdx <= 12; bIdx += 4) {
    c = 0.0;
    aIdx = bIdx + 4;
    for (b_exitflag = bIdx + 1; b_exitflag <= aIdx; b_exitflag++) {
      c += JacCeqTrans[b_exitflag - 1] * fval[(b_exitflag - bIdx) - 1];
    }
    b_exitflag = bIdx >> 2;
    gradf[b_exitflag] += c;
  }
  c = 0.0;
  stepSuccessful = true;
  normGradF = 0.0;
  t = fabs(gradf[0]);
  evalOK = rtIsNaN(t);
  if (evalOK || (t > 0.0)) {
    c = t;
  }
  if (evalOK || (t > 0.0)) {
    normGradF = t;
  }
  t = fabs(gradf[1]);
  evalOK = rtIsNaN(t);
  if (evalOK || (t > c)) {
    c = t;
  }
  if (evalOK || (t > normGradF)) {
    normGradF = t;
  }
  t = fabs(gradf[2]);
  evalOK = rtIsNaN(t);
  if (evalOK || (t > c)) {
    c = t;
  }
  if (evalOK || (t > normGradF)) {
    normGradF = t;
  }
  t = fabs(gradf[3]);
  evalOK = rtIsNaN(t);
  if (evalOK || (t > c)) {
    c = t;
  }
  if (evalOK || (t > normGradF)) {
    normGradF = t;
  }
  relFactor = fmax(c, 1.0);
  if (normGradF <= 1.0E-10 * relFactor) {
    b_exitflag = 1;
  } else if (FiniteDifferences.numEvals + 1 >= 800) {
    b_exitflag = 0;
  } else {
    b_exitflag = -5;
  }
  exitg1 = false;
  while ((!exitg1) && (b_exitflag == -5)) {
    double fNew[4];
    double d_f_temp_tmp;
    double e_f_temp_tmp;
    double f_f_temp_tmp;
    double g_f_temp_tmp;
    double xp_idx_0;
    double xp_idx_1;
    double xp_idx_2;
    double xp_idx_3;
    boolean_T guard1 = false;
    rhs[0] = -fval[0];
    rhs[4] = 0.0;
    rhs[1] = -fval[1];
    rhs[5] = 0.0;
    rhs[2] = -fval[2];
    rhs[6] = 0.0;
    rhs[3] = -fval[3];
    rhs[7] = 0.0;
    linearLeastSquares(augJacobian, rhs, dx);
    xp_idx_0 = x[0] + dx[0];
    xp_idx_1 = x[1] + dx[1];
    xp_idx_2 = x[2] + dx[2];
    xp_idx_3 = x[3] + dx[3];
    /*  确保 F 是 1x4 实数数组 */
    /*  T2/T1 输入 */
    /*  T3/T1 输入 */
    c = xp_idx_3 * xp_idx_3;
    t = xp_idx_0 * xp_idx_0;
    normGradF = t + c;
    c_f_temp_tmp = xp_idx_1 * xp_idx_1;
    d_f_temp_tmp = c_f_temp_tmp + c;
    f_temp[0] =
        f_temp_tmp - xp_idx_0 / xp_idx_1 * sqrt(normGradF / d_f_temp_tmp);
    e_f_temp_tmp = xp_idx_2 * xp_idx_2;
    f_temp[1] = b_f_temp_tmp -
                xp_idx_0 / xp_idx_2 * sqrt(normGradF / (e_f_temp_tmp + c));
    f_f_temp_tmp = sqrt(normGradF);
    g_f_temp_tmp = rt_powd_snf(xp_idx_3, 4.0);
    f_temp[2] = ((g_f_temp_tmp / t / normGradF + 2.0) * cos(xp_idx_0) *
                     cosh(f_f_temp_tmp) +
                 2.0) +
                e_f_temp_tmp / xp_idx_0 / f_f_temp_tmp * sin(xp_idx_0) *
                    sinh(f_f_temp_tmp);
    normGradF = sqrt(d_f_temp_tmp);
    f_temp[3] = ((g_f_temp_tmp / c_f_temp_tmp / d_f_temp_tmp + 2.0) *
                     cos(xp_idx_1) * cosh(normGradF) +
                 2.0) +
                c / xp_idx_1 / normGradF * sin(xp_idx_1) * sinh(normGradF);
    fNew[0] = f_temp[0];
    evalOK = ((!rtIsInf(f_temp[0])) && (!rtIsNaN(f_temp[0])));
    fNew[1] = f_temp[1];
    if ((!evalOK) || (rtIsInf(f_temp[1]) || rtIsNaN(f_temp[1]))) {
      evalOK = false;
    }
    fNew[2] = f_temp[2];
    if ((!evalOK) || (rtIsInf(f_temp[2]) || rtIsNaN(f_temp[2]))) {
      evalOK = false;
    }
    fNew[3] = f_temp[3];
    c = ((f_temp[0] * f_temp[0] + f_temp[1] * f_temp[1]) +
         f_temp[2] * f_temp[2]) +
        f_temp[3] * f_temp[3];
    if ((!evalOK) || (rtIsInf(f_temp[3]) || rtIsNaN(f_temp[3]))) {
      evalOK = false;
    }
    funcCount++;
    guard1 = false;
    if ((c < resnorm) && evalOK) {
      iter++;
      funDiff = fabs(c - resnorm) / resnorm;
      resnorm = c;
      a__3[0] = xp_idx_0;
      a__3[1] = xp_idx_1;
      a__3[2] = xp_idx_2;
      a__3[3] = xp_idx_3;
      b_FiniteDifferences = FiniteDifferences;
      evalOK = computeFiniteDifferences(&b_FiniteDifferences, fNew, a__3,
                                        JacCeqTrans);
      funcCount += b_FiniteDifferences.numEvals;
      for (aIdx = 0; aIdx < 4; aIdx++) {
        b_exitflag = aIdx << 3;
        augJacobian[b_exitflag] = JacCeqTrans[aIdx];
        augJacobian[b_exitflag + 1] = JacCeqTrans[aIdx + 4];
        augJacobian[b_exitflag + 2] = JacCeqTrans[aIdx + 8];
        augJacobian[b_exitflag + 3] = JacCeqTrans[aIdx + 12];
        fval[aIdx] = fNew[aIdx];
      }
      for (b_exitflag = 0; b_exitflag < 4; b_exitflag++) {
        aIdx = b_exitflag << 3;
        bIdx = b_exitflag << 2;
        JacCeqTrans[bIdx] = augJacobian[aIdx];
        JacCeqTrans[bIdx + 1] = augJacobian[aIdx + 1];
        JacCeqTrans[bIdx + 2] = augJacobian[aIdx + 2];
        JacCeqTrans[bIdx + 3] = augJacobian[aIdx + 3];
      }
      if (evalOK) {
        x[0] = xp_idx_0;
        x[1] = xp_idx_1;
        x[2] = xp_idx_2;
        x[3] = xp_idx_3;
        if (stepSuccessful) {
          b_gamma *= 0.1;
        }
        stepSuccessful = true;
        guard1 = true;
      } else {
        b_exitflag = 2;
        exitg1 = true;
      }
    } else {
      b_gamma *= 10.0;
      stepSuccessful = false;
      for (b_exitflag = 0; b_exitflag < 4; b_exitflag++) {
        aIdx = b_exitflag << 2;
        bIdx = b_exitflag << 3;
        augJacobian[bIdx] = JacCeqTrans[aIdx];
        augJacobian[bIdx + 1] = JacCeqTrans[aIdx + 1];
        augJacobian[bIdx + 2] = JacCeqTrans[aIdx + 2];
        augJacobian[bIdx + 3] = JacCeqTrans[aIdx + 3];
      }
      guard1 = true;
    }
    if (guard1) {
      c = sqrt(b_gamma);
      for (bIdx = 0; bIdx < 4; bIdx++) {
        aIdx = (bIdx + 1) << 3;
        augJacobian[aIdx - 4] = 0.0;
        augJacobian[aIdx - 3] = 0.0;
        augJacobian[aIdx - 2] = 0.0;
        augJacobian[aIdx - 1] = 0.0;
        augJacobian[(bIdx + (bIdx << 3)) + 4] = c;
        gradf[bIdx] = 0.0;
      }
      for (bIdx = 0; bIdx <= 12; bIdx += 4) {
        c = 0.0;
        aIdx = bIdx + 4;
        for (b_exitflag = bIdx + 1; b_exitflag <= aIdx; b_exitflag++) {
          c += JacCeqTrans[b_exitflag - 1] * fval[(b_exitflag - bIdx) - 1];
        }
        b_exitflag = bIdx >> 2;
        gradf[b_exitflag] += c;
      }
      b_exitflag = checkStoppingCriteria(gradf, relFactor, funDiff, x, dx,
                                         funcCount, stepSuccessful, &iter);
      if (b_exitflag != -5) {
        exitg1 = true;
      }
    }
  }
  *output_firstorderopt = 0.0;
  *output_iterations = iter;
  *output_funcCount = funcCount;
  normGradF = 3.3121686421112381E-170;
  c = fabs(gradf[0]);
  if (rtIsNaN(c) || (c > 0.0)) {
    *output_firstorderopt = c;
  }
  c = fabs(dx[0]);
  if (c > 3.3121686421112381E-170) {
    c_f_temp_tmp = 1.0;
    normGradF = c;
  } else {
    t = c / 3.3121686421112381E-170;
    c_f_temp_tmp = t * t;
  }
  c = fabs(gradf[1]);
  if (rtIsNaN(c) || (c > *output_firstorderopt)) {
    *output_firstorderopt = c;
  }
  c = fabs(dx[1]);
  if (c > normGradF) {
    t = normGradF / c;
    c_f_temp_tmp = c_f_temp_tmp * t * t + 1.0;
    normGradF = c;
  } else {
    t = c / normGradF;
    c_f_temp_tmp += t * t;
  }
  c = fabs(gradf[2]);
  if (rtIsNaN(c) || (c > *output_firstorderopt)) {
    *output_firstorderopt = c;
  }
  c = fabs(dx[2]);
  if (c > normGradF) {
    t = normGradF / c;
    c_f_temp_tmp = c_f_temp_tmp * t * t + 1.0;
    normGradF = c;
  } else {
    t = c / normGradF;
    c_f_temp_tmp += t * t;
  }
  c = fabs(gradf[3]);
  if (rtIsNaN(c) || (c > *output_firstorderopt)) {
    *output_firstorderopt = c;
  }
  c = fabs(dx[3]);
  if (c > normGradF) {
    t = normGradF / c;
    c_f_temp_tmp = c_f_temp_tmp * t * t + 1.0;
    normGradF = c;
  } else {
    t = c / normGradF;
    c_f_temp_tmp += t * t;
  }
  *output_stepsize = normGradF * sqrt(c_f_temp_tmp);
  exitflag = b_exitflag;
  if ((b_exitflag > 0) && (resnorm > 0.001)) {
    exitflag = -2.0;
  }
  return exitflag;
}

/* End of code generation (fsolve.c) */
